
#include "clustering.h"

Clustering::Clustering(): lista_size (100000) {
   
   model = 0;
   init();
}

Clustering::Clustering(Mesh *m): lista_size (100000) {
   
   //~ model = m;
   init();
}

Clustering::Clustering(Mesh *m, Skeleton *s) : lista_size (100000) {
   
   
   //~ model = m;
   //~ stack_search.resize(s->nBones()); // 
   //~ lflist_bones.resize(s.nJoints());
   init();
}

Clustering::Clustering(MeshWithSkeleton *m) : lista_size (100000) {
   
   cout<<"Clustering .... constructor begin"<<endl;
   
   model = m;
   //~ stack_search.resize(model->nBones()); // 
   //~ lflist_bones.resize(model->nJoints());
   init();
   
   cout<<"Clustering .... constructor end"<<endl;
}
   
Clustering::~Clustering() {
   
}

void Clustering::init() {
   
   //~ lista_size=100000;
   inter_test_limit = 0.2; 
   
   infinPoint = Point3(10000,10000,10000);
   current_basis = infinPoint; // 
   Dbasis_to_i2 = -1E8;
   
   // using joints
   // curr_bas_joint = &infinPoint[0];
   curr_bas_joint = infinPoint;
   
   lista.resize(lista_size);
}

void Clustering::execute() {
   
   init();
   i1bone = 1; // because bone 0 is invalid
   
   cout<<"riggingControl() begin execute with bonesssssssssssssssssssssssss"<<endl;
   assert(model != 0);
   
   MeshWithSkeleton::Vertex_iterator vertex = model->vertices_begin();
   //~ int component_list[component_list_size];
   
   cout<<"getClosestBone() "<<endl;
   
/////////   curr_bas_joint = &model->skeleton->joints[0][0]; // apontando para o primeiro vertice
   //~ current_basis = &model->vpositions[0]; // apontando para o primeiro vertice
   current_basis = vertex[0].point(); // apontando para o primeiro vertice
   vertex = model->vertices_begin();
   while (vertex < model->vertices_end()) {
      vertex->reset_clustering();
      ++vertex;
   }
   vertex = model->vertices_begin();
   while (vertex < model->vertices_end()) {
      getClosestBone(vertex);
      ++vertex;
   }
   
   cout<<"riggingControl() 1111"<<endl;
   
   vertex = model->vertices_begin();
   while (vertex < model->vertices_end()) {
      getClosestVisibleBone(vertex);
      ++vertex;
   }
   
   
   cout<<"riggingControl() 22222222"<<endl;
   
   //~ for (int i=0; i<model->skeleton->closest_vertex.size(); ++i) {
      //~ cout<<"::: "<<model->skeleton->closest_vertex[i]<<endl;
   //~ }
   
   cout<<"  1111 component_list.size(): "<<component_list.size()<<endl;
   cout<<"  1111 global_stack.size(): "<<global_stack.size()<<endl;
   
   int current_component = -1;
   vertex = model->vertices_begin();
   for (int i=1; i<model->skeleton->nBones(); ++i) {
      if (vertex[model->skeleton->closest_vertex[i]].rigging_group == i) {
         current_component++;
         vertex[model->skeleton->closest_vertex[i]].assigned = true;
         int r = expand_Component(model->skeleton->closest_vertex[i], current_component);
         component_list.push_back(r);
         //~ cout<<"     for global_stack.size(): "<<i<<" : "<<r<<" : "<<global_stack.size()<<endl;
      }
   }
///// ////   closest_components = current_component + 1;
   
   cout<<"  2222 component_list.size(): "<<component_list.size()<<endl;
   cout<<"  2222 global_stack.size(): "<<global_stack.size()<<endl;
   
   vertex = model->vertices_begin();
   for (int i=0; i<model->nvertices(); ++i) {
      if ((vertex[i].rigging_group != -1) and (not vertex[i].assigned)) {
         current_component++;
         vertex[i].assigned = true;
         int id = expand_Component(i,current_component);
         //~ cout<<"id: "<<id<<" : "<<i<<endl;
         component_list.push_back(id);
      }
   }
   
   cout<<"  3333 component_list.size(): "<<component_list.size()<<endl;
   cout<<"  2222 global_stack.size(): "<<global_stack.size()<<endl;
   
   copy(component_list.begin(), component_list.end(), ostream_iterator<int>(cout, " "));
   
   cout<<"riggingControl() 33333333"<<endl;
   
   //~ vertex = model->vertices_begin();
   //~ while (vertex < model->vertices_end()) {
      //~ cout<<"xxx vertex->rigging_group: "<<vertex->rigging_group<<endl;
      //~ ++vertex;
   //~ }
   
   //~ cout<<"vertex[0]: "<<vertex[0].next_in_component<<" : "<<vertex[0].rigging_group<<endl;
   //~ cout<<"vertex[1]: "<<vertex[1].next_in_component<<" : "<<vertex[1].rigging_group<<endl;
   //~ cout<<"vertex[900]: "<<vertex[900].next_in_component<<" : "<<vertex[900].rigging_group<<endl;
   
   //~ exit(0);
   
   //~ cout<<"component_list component_list"<<endl;
   //~ for (int i=0; i<component_list.size(); ++i) {
      //~ cout<<"::: "<<component_list[i]<<endl;
   //~ }
   
   
   int xtriang_vertex;
   int current_initial_vertex, current_vertex;
   //~ for (int j=closest_components; j< current_component; ++j) {
   //~ for (int j=0; j< current_component; ++j) {
   for (unsigned int j=0; j<component_list.size(); ++j) {
      bool visible_component = false;
      xtriang_vertex = -1;
      current_initial_vertex = component_list[j];
      
      const Point3 &p = vertex[current_initial_vertex].point();
      Vector3 vd = (model->skeleton->getBoneProjector(&p[0], vertex[current_initial_vertex].rigging_group))*(-1.0);
      Vector3 c = Vector3(p[0],p[1],p[2]) + vd;
      
      xtriang_vertex = Initial_vertex_invisible_bonejoint(current_initial_vertex, vd, c);
      //~ xtriang_vertex = Initial_vertex_invisible(current_initial_vertex);
      
      //~ cout<<"~~ current_initial_vertex: "<<current_initial_vertex<<endl;
      //~ cout<<"~~ vertex[current_initial_vertex].rigging_group: "<<vertex[current_initial_vertex].rigging_group<<endl;
      //~ Number *p = &model->vpositions[3*component_list[j]];
      //~ cout<<"~~p: "<<p[0]<<" "<<p[1]<<" "<<p[2]<<endl;
      //~ cout<<"~~ Initial_vertex_invisible() returned value = : "<<xtriang_vertex<<" "<<endl;
      
      //~ cout<<"j: "<<j<<endl;
      
//      int conta=0;
      if (xtriang_vertex != -1) {
         //~ cout<<"<><><> ooo "<<current_initial_vertex<<endl;
         current_vertex = vertex[current_initial_vertex].next_in_component;
         //~ cout<<"uuu "<<current_vertex<<endl;
         while(current_vertex != -1) {
            //~ cout<<">>>::::: "<<current_vertex<<endl;
            //~ if (conta>100) { cout<<"exit()"<<endl; exit(0);}
            //~ cout<<"xxx"<<endl;
            //~ xtriang_vertex = Vertex_invisible(current_vertex, xtriang_vertex);
            
            const Point3 &p = vertex[current_vertex].point();
            Vector3 vd = (model->skeleton->getBoneProjector(&p[0], vertex[current_vertex].rigging_group))*(-1.0);
            Vector3 c = Vector3(p[0]+vd[0],p[1]+vd[1],p[2]+vd[2]);
            
            xtriang_vertex = Vertex_invisible_bonejoint(current_vertex, xtriang_vertex, vd, c);
            
            //~ cout<<" ... Vertex_invisible() returned value: "<<xtriang_vertex<<" "<<endl;
            if (xtriang_vertex != -1) {
               current_vertex = vertex[current_vertex].next_in_component;
            }
            else {
               //~ cout<<"aaa"<<endl;
               //~ xtriang_vertex = Initial_vertex_invisible(current_vertex);
               
               const Point3 &p = vertex[current_vertex].point();
               Vector3 vd = (model->skeleton->getBoneProjector(&p[0], vertex[current_vertex].rigging_group))*(-1.0);
               Vector3 c = Vector3(p[0],p[1],p[2]) + vd;
               
               xtriang_vertex = Initial_vertex_invisible_bonejoint(current_vertex, vd, c);
               
               //~ cout<<" ... Initial_vertex_invisible() returned value: "<<xtriang_vertex<<" "<<endl;
               if (xtriang_vertex != -1) {
                  current_vertex = vertex[current_vertex].next_in_component;
               }
               else {
                  visible_component = true;
                  //~ cout<<"XXXXXXXXXX XXXXXXXXXXXX XXXXXXXXXXXXXXXXXX XXXXXXXXXXXXX"<<endl;
                  break;
               }
            }
         }
         //~ cout<<"pppppppp"<<endl;
      }
      else {
         visible_component = true;
      }
      
      if (!visible_component) {
         int cont = component_list[j];
         do {
            vertex[cont].rigging_group = -1;
            vertex[cont].dClosestJoint = 1e8;
            cont = vertex[cont].next_in_component;
         } while (cont != -1);
         component_list[j] = -1;
      }
   }
   component_list.push_back(-2);
   //~ cout<<"wwwwwwww"<<endl;
   
   
   
   //~ vertex = model->vertices_begin();
   //~ cout<<"component_list component_list depois"<<endl;
   //~ for (int i=0; i<component_list.size(); ++i) {
      //~ if (component_list[i] == -1) continue;
      //~ cout<<"antes do propag::: "<<component_list[i]<<endl;
      //~ Number *p = &model->vpositions[3*component_list[i]];
      //~ cout<<" p: "<<p[0]<<" "<<p[1]<<" "<<p[2]<<endl;
   //~ }
   
   
   //~ cout<<"modificando! .. component_list.size(): "<<component_list.size()<<endl;
   //~ vertex = model->vertices_begin();
   //~ for (int i=0; i<model->nvertices(); ++i) {
      //~ ////~ cout<<"<> "<<i<<" <> "<<vertex[i].rigging_group<<endl;
      //~ cout<<"<> "<<i<<" <> "<<vertex[i].curr_comp<<endl;
      //~ if (component_list[vertex[i].curr_comp] == -1 ) {
         //~ vertex[i].rigging_group = 0;
      //~ }
   //~ }
   
   
   cout<<"propagate_primary_skin()"<<endl;
   propagate_primary_skin();
   
   //~ cout<<"uuuuuuuuuu"<<endl;
   
   //~ vertex = model->vertices_begin();
   //~ while (vertex < model->vertices_end()) {
      //~ if (vertex->rigging_group == -1) cout<<"vertex->id(): "<<vertex->id()/3<<endl;
      //~ ++vertex;
   //~ }
   
   //~ cout<<"CGAL::sign(0): "<<CGAL::sign(0)<<endl;
   
   Rigging_Validation();
   
}

void Clustering::execute_joints() {
   
   cout<<"riggingControl() begin excute with jointts"<<endl;
   assert(model != 0);
   
   init();
   i1joint = 0;
   
   MeshWithSkeleton::Vertex_iterator vertex = model->vertices_begin();
   
   cout<<"getClosestJoint() "<<endl;
   
   vertex = model->vertices_begin();
   while (vertex < model->vertices_end()) {
      vertex->reset_clustering();
      ++vertex;
   }
   
   curr_bas_joint = model->skeleton->joints[0]; // apontando para o primeiro joint
   current_basis = vertex[0].point(); // apontando para o primeiro vertice
   vertex = model->vertices_begin();
   while (vertex < model->vertices_end()) {
      getClosestJoint(vertex);
      ++vertex;
   }
   
   cout<<"riggingControl() 1111"<<endl;
   
   vertex = model->vertices_begin();
   while (vertex < model->vertices_end()) {
      getClosestVisibleJoint(vertex);
      ++vertex;
   }
   
   /*
   cout<<"riggingControl() 22222222"<<endl;
   
   //~ for (int i=0; i<model->skeleton->closest_vertex.size(); ++i) {
      //~ cout<<"::: "<<model->skeleton->closest_vertex[i]<<endl;
   //~ }
   //~ cout<<"component_list.size(): "<<component_list.size()<<endl;
   
   int current_component = -1;
   vertex = model->vertices_begin();
   for (unsigned int i=1; i<model->skeleton->nBones(); ++i) {
      if (vertex[model->skeleton->closest_vertex[i]].rigging_group == (int)i) {
         current_component++;
         vertex[model->skeleton->closest_vertex[i]].assigned = true;
         component_list.push_back(expand_Component(model->skeleton->closest_vertex[i], current_component));
      }
   }
///// ////   closest_components = current_component + 1;
   
   vertex = model->vertices_begin();
   for (unsigned int i=0; i<model->nvertices(); ++i) {
      if ((vertex[i].rigging_group != -1) and (not vertex[i].assigned)) {
         current_component++;
         vertex[i].assigned = true;
         int id = expand_Component(i,current_component);
         //~ cout<<"id: "<<id<<" : "<<i<<endl;
         component_list.push_back(id);
      }
   }
   
   
   cout<<"riggingControl() 33333333"<<endl;
   
   //~ vertex = model->vertices_begin();
   //~ while (vertex < model->vertices_end()) {
      //~ cout<<"xxx vertex->rigging_group: "<<vertex->rigging_group<<endl;
      //~ ++vertex;
   //~ }
   
   //~ cout<<"vertex[0]: "<<vertex[0].next_in_component<<" : "<<vertex[0].rigging_group<<endl;
   //~ cout<<"vertex[1]: "<<vertex[1].next_in_component<<" : "<<vertex[1].rigging_group<<endl;
   //~ cout<<"vertex[900]: "<<vertex[900].next_in_component<<" : "<<vertex[900].rigging_group<<endl;
   
   //~ exit(0);
   
   //~ cout<<"component_list component_list"<<endl;
   //~ for (int i=0; i<component_list.size(); ++i) {
      //~ cout<<"::: "<<component_list[i]<<endl;
   //~ }
   
   int xtriang_vertex;
   int current_initial_vertex, current_vertex;
   //~ for (int j=closest_components; j< current_component; ++j) {
   //~ for (int j=0; j< current_component; ++j) {
   for (unsigned int j=0; j<component_list.size(); ++j) {
      bool visible_component = false;
      xtriang_vertex = -1;
      current_initial_vertex = component_list[j];
      
      xtriang_vertex = Initial_vertex_invisible_bone(current_initial_vertex);
      //~ xtriang_vertex = Initial_vertex_invisible(current_initial_vertex);
      
      //~ cout<<"~~ current_initial_vertex: "<<current_initial_vertex<<endl;
      //~ cout<<"~~ vertex[current_initial_vertex].rigging_group: "<<vertex[current_initial_vertex].rigging_group<<endl;
      //~ Number *p = &model->vpositions[3*component_list[j]];
      //~ cout<<"~~p: "<<p[0]<<" "<<p[1]<<" "<<p[2]<<endl;
      //~ cout<<"~~ Initial_vertex_invisible() returned value = : "<<xtriang_vertex<<" "<<endl;
      
      //~ cout<<"j: "<<j<<endl;
      
      //int conta=0;
      if (xtriang_vertex != -1) {
         //~ cout<<"<><><> ooo "<<current_initial_vertex<<endl;
         current_vertex = vertex[current_initial_vertex].next_in_component;
         //~ cout<<"uuu "<<current_vertex<<endl;
         while(current_vertex != -1) {
            //~ cout<<">>>::::: "<<current_vertex<<endl;
            //~ if (conta>100) { cout<<"exit()"<<endl; exit(0);}
            //~ cout<<"xxx"<<endl;
            //~ xtriang_vertex = Vertex_invisible(current_vertex, xtriang_vertex);
            xtriang_vertex = Vertex_invisible_bone(current_vertex, xtriang_vertex);
            
            //~ cout<<" ... Vertex_invisible() returned value: "<<xtriang_vertex<<" "<<endl;
            if (xtriang_vertex != -1) {
               current_vertex = vertex[current_vertex].next_in_component;
            }
            else {
               //~ cout<<"aaa"<<endl;
               //~ xtriang_vertex = Initial_vertex_invisible(current_vertex);
               xtriang_vertex = Initial_vertex_invisible_bone(current_vertex);
               //~ cout<<" ... Initial_vertex_invisible() returned value: "<<xtriang_vertex<<" "<<endl;
               if (xtriang_vertex != -1) {
                  current_vertex = vertex[current_vertex].next_in_component;
               }
               else {
                  visible_component = true;
                  //~ cout<<"XXXXXXXXXX XXXXXXXXXXXX XXXXXXXXXXXXXXXXXX XXXXXXXXXXXXX"<<endl;
                  break;
               }
            }
         }
         //~ cout<<"pppppppp"<<endl;
      }
      else {
         visible_component = true;
      }
      
      if (!visible_component) {
         int cont = component_list[j];
         do {
            vertex[cont].rigging_group = -1;
            vertex[cont].dClosestJoint = 1e8;
            cont = vertex[cont].next_in_component;
         } while (cont != -1);
         component_list[j] = -1;
      }
   }
   component_list.push_back(-2);
   //~ cout<<"wwwwwwww"<<endl;
   
   
   //~ vertex = model->vertices_begin();
   //~ cout<<"component_list component_list depois"<<endl;
   //~ for (int i=0; i<component_list.size(); ++i) {
      //~ if (component_list[i] == -1) continue;
      //~ cout<<"antes do propag::: "<<component_list[i]<<endl;
      //~ Number *p = &model->vpositions[3*component_list[i]];
      //~ cout<<" p: "<<p[0]<<" "<<p[1]<<" "<<p[2]<<endl;
   //~ }
   
   
   //~ cout<<"modificando! .. component_list.size(): "<<component_list.size()<<endl;
   //~ vertex = model->vertices_begin();
   //~ for (int i=0; i<model->nvertices(); ++i) {
      //~ ////~ cout<<"<> "<<i<<" <> "<<vertex[i].rigging_group<<endl;
      //~ cout<<"<> "<<i<<" <> "<<vertex[i].curr_comp<<endl;
      //~ if (component_list[vertex[i].curr_comp] == -1 ) {
         //~ vertex[i].rigging_group = 0;
      //~ }
   //~ }
   
   
   cout<<"propagate_primary_skin()"<<endl;
   
   propagate_primary_skin();
   
   //~ cout<<"uuuuuuuuuu"<<endl;
   
   //~ vertex = model->vertices_begin();
   //~ while (vertex < model->vertices_end()) {
      //~ if (vertex->rigging_group == -1) cout<<"vertex->id(): "<<vertex->id()/3<<endl;
      //~ ++vertex;
   //~ }
   
   cout<<"CGAL::sign(0): "<<CGAL::sign(0)<<endl;
   */

}

Number Clustering::length(int id_he) const {
   
   MeshWithSkeleton::Halfedge_iterator edge = model->halfedges_begin();
   //~ Number *pA = &model->i_vpositions[edge[id_he].vertex()->id()]; 
   //~ Number *pB = &model->i_vpositions[edge[id_he].opposite()->vertex()->id()]; 
   
   const Point3 &pA = edge[id_he].vertex()->point();
   const Point3 &pB = edge[id_he].opposite()->vertex()->point();
   
   const Number &vx = pB[0]-pA[0];
   const Number &vy = pB[1]-pA[1];
   const Number &vz = pB[2]-pA[2];
   
   return sqrt(vx*vx + vy*vy + vz*vz);
}

bool Clustering::interTriSeg(const Vector3 &u, const Vector3 &v, const Vector3 &dir, const Vector3 &w0) {
   
   Vector3  n;             // triangle vectors
   Vector3  w;             // ray vectors
   Vector3  intersection;  // intersection of ray and plane
   Number   r, a, b;       // params to calc ray-plane intersect
   Number   SMALL_NUM = 1E-8;
   
   // get triangle edge vectors and plane normal
   n = cross_product(u,v);             // cross product
   if (n * n == 0)                    // triangle is degenerate
       return false;                 // another triangle will contain the intersection if
                                       //there is one.
   
   a = n*w0;
   b = n*dir;
   if (fabs(b) < SMALL_NUM) {     // ray is parallel to triangle plane
       return false;             // ray disjoint from plane
   }

   // get intersect point of ray with triangle plane
   r = a/b;
   //~ cout<<"inter r: "<<r<<" a: "<<a<<" b: "<<b<<endl;
   if (r < 0.0 || r > 1.0)                   // segment not intersect triangle plane
       return false;

   intersection = w0 - r*dir;           // intersect point of ray and plane
   //~ cout<<"ififs "<<endl;
   // is I inside T?
   Number t1, t2;
   float Det = u[0]*v[1] - u[1]*v[0]; 
   if (fabs(Det) > SMALL_NUM) {
      t1 = (intersection[0]*v[1] - intersection[1]*v[0])/Det;
      //~ cout<<"  1 t1: "<<t1<<endl;
      if(t1 < 0 || t1 > 1) return false;
      t2 = (intersection[1]*u[0] - intersection[0]*u[1])/Det;
      //~ cout<<"  1 t2: "<<t2<<endl;
      if(t2 < 0 || t2 > 1 || (t1+t2)>1 ) return false;
      return true;
   }
   Det = u[0]*v[2] - u[2]*v[0];
   if (fabs(Det) > SMALL_NUM) {
      t1 = (intersection[0]*v[2] - intersection[2]*v[0])/Det;
      //~ cout<<"  2 t1: "<<t1<<endl;
      if(t1 < 0 || t1 > 1) return false;
      t2 = (intersection[2]*u[0] - intersection[0]*u[2])/Det;
      //~ cout<<"  2 t2: "<<t2<<endl;
      if(t2 < 0 || t2 > 1  || (t1+t2)>1 ) return false;
      return true;
   }
   Det = u[1]*v[2] - u[2]*v[1];
   t1 = (intersection[1]*v[2] - intersection[2]*v[1])/Det;
   //~ cout<<"  3 t1: "<<t1<<endl;
   if(t1 < 0 || t1 > 1) return false;
   t2 = (intersection[2]*u[1] - intersection[1]*u[2])/Det;
   //~ cout<<"  3 t2: "<<t2<<endl;
   if(t2 < 0 || t2 > 1  || (t1+t2)>1 ) return false;
   return true;
}

void Clustering::getClosestBone(MeshWithSkeleton::Vertex_handle vertex) {
   
   /*
   Number t1;
   int v = vertex->id();
   int i; Number t;
   Number *p = &model->vpositions[v];
   
   //~ Vector3 u1(p[0]-curr_bas_joint[0], p[1]-curr_bas_joint[1], p[2]-curr_bas_joint[2]);
   Vector3 u2(p[0]-current_basis[0], p[1]-current_basis[1], p[2]-current_basis[2]);
   Number D_to_i1 = model->skeleton->getDistance(p, i1bone, t1);
   Number D_to_basis = sqrt(u2*u2);
   if ((Dbasis_to_i2 - D_to_basis) < D_to_i1) {
      Dbasis_to_i2 = 1e8;
      int iterm = i1bone;
      for (i=1; i<iterm; ++i) {
         //~ const Point3 &c = model->skeleton->joints[i];
         //~ Vector3 vd(p[0]-c[0], p[1]-c[1], p[2]-c[2]);
         Number d = model->skeleton->getDistance(p, i, t);
         if (d < Dbasis_to_i2){
            if (d < D_to_i1) {
               Dbasis_to_i2 = D_to_i1;
               D_to_i1 = d;
               i1bone = i;
               t1 = t;
            }
            else{
               Dbasis_to_i2 = d;
            }
         }
      }
      i++;
      for (int j=i; j<model->skeleton->nBones(); ++j) {
         //~ const Point3 &c = model->skeleton->joints[j];
         //~ Vector3 vd(p[0]-c[0], p[1]-c[1], p[2]-c[2]);
         Number d = model->skeleton->getDistance(p, j, t);
         if (d < Dbasis_to_i2){
            if (d < D_to_i1) {
               Dbasis_to_i2 = D_to_i1;
               D_to_i1 = d;
               i1bone = j;
               t1 = t;
            }
            else {
               Dbasis_to_i2 = d;
            }
         }
      }
      current_basis = p;
   }
   
   vertex->rigging_group = i1bone;
   vertex->dClosestJoint = D_to_i1;
   vertex->tclosest_bone = t1;
   
   if (D_to_i1 < model->skeleton->min_distance_vertex[i1bone]) {
      model->skeleton->min_distance_vertex[i1bone] = D_to_i1;
      model->skeleton->closest_vertex[i1bone] = v/3;
   }
   */
   /// 
   
   //~ Number *p = &model->vpositions[vertex->id()];
   const Point3 &p = vertex->point();
   Number D1=1e8;
   for (unsigned int ii=1; ii<model->skeleton->nBones(); ++ii) {
      float t;
      Number d = model->skeleton->getDistance(&p[0], ii, t);
      
      if (d<D1) {
         D1 = d;
         vertex->rigging_group = ii;
         vertex->tclosest_bone = t;
      }
   }
   
   vertex->dClosestJoint = D1;
   if (D1 < model->skeleton->min_distance_vertex[vertex->rigging_group]) {
      model->skeleton->min_distance_vertex[vertex->rigging_group] = D1;
      model->skeleton->closest_vertex[vertex->rigging_group] = vertex->id();
   }
}

void Clustering::getClosestVisibleBone(MeshWithSkeleton::Vertex_handle vertex) {
   /// 
   const Point3 &p = vertex->point();
   const Vector3 &n = vertex->normal();
   
   const Bone &bo = model->skeleton->bones[vertex->rigging_group];
   const Point3 &b = model->skeleton->joints[bo.first];
   const Point3 &a = model->skeleton->joints[bo.second];
   Vector3 vd = Vector3(p[0],p[1],p[2]) - 
               (Vector3(b[0],b[1],b[2])*vertex->tclosest_bone + 
                Vector3(a[0],a[1],a[2])*(1.0-vertex->tclosest_bone));
   if (vd*n <= 0) {
      Number D1=1e8; 
      int rigginggroup = vertex->rigging_group;
      Number possibleVertexdClosestJoint = 1e8;
      int possibleVertexRigging_group = -1; // cuidar del bone invalido 0
      int i;
      for (i=1; i<rigginggroup; i++) {
         Vector3 vd = model->skeleton->getBoneProjector(&p[0], i);
         Number d = sqrt(vd*vd);
         if (vd*n > 0) {
            if (d<D1) {
               possibleVertexdClosestJoint = d;
               possibleVertexRigging_group = i;
               D1 = d;
            }
         }
      }
      i++;
      for (unsigned int j=i; j<model->skeleton->nBones(); j++) {
         Vector3 vd = model->skeleton->getBoneProjector(&p[0], j);
         Number d = sqrt(vd*vd);
         if (vd*n > 0) {
            if (d<D1) {
               possibleVertexdClosestJoint = d;
               possibleVertexRigging_group = j;
               D1 = d;
            }
         }
      }
      if (possibleVertexRigging_group == -1) {
         vertex->rigging_group = -1;
         vertex->dClosestJoint = 1e8;
         return;
      }
      
      const Number EPSILON=1e-6;
      
      float t, tt;
      Number d = model->skeleton->getDistance(&p[0], possibleVertexRigging_group, t);
      const Bone &b = model->skeleton->bones[possibleVertexRigging_group];
      if (t>EPSILON) {
         const Point3 &bb = model->skeleton->joints[b.first];
         Number dd = model->skeleton->getDistance(&bb[0], rigginggroup, tt);
         if (t<1-EPSILON or dd>EPSILON) {
            vertex->dClosestJoint = possibleVertexdClosestJoint;
            vertex->rigging_group = possibleVertexRigging_group; 
         }
      }
      else {
         const Point3 &aa = model->skeleton->joints[b.second];
         Number dd = model->skeleton->getDistance(&aa[0], rigginggroup, tt);
         if (dd>EPSILON) {
            vertex->dClosestJoint = possibleVertexdClosestJoint;
            vertex->rigging_group = possibleVertexRigging_group; 
         }
      }
   }
   
}

int Clustering::expand_Component(int v, int current_component) {
   
   int current_edge, current_vertex;
   MeshWithSkeleton::Vertex_iterator   vertex = model->vertices_begin();
   MeshWithSkeleton::Halfedge_iterator edge = model->halfedges_begin();
   
   int last_vertex_in_component = v;
   vertex[v].next_in_component = -1;
   int current_bone = vertex[v].rigging_group;
   
   int n_listed = 1;
   int n_explored = 0;
   
   lista[0].e_ini = vertex[v].halfedge()->opposite()->hedgeid; //
   lista[0].e_term = vertex[v].halfedge()->opposite()->hedgeid;
   
   //~ int contador=1;
   //~ int contadorpilha=0;
   
   while (n_listed > n_explored) {
      current_edge = lista[n_explored].e_ini;
      //~ cout<<" : "<<current_edge;
      int edge_ant = edge[current_edge].next()->hedgeid;
      do {
         current_vertex = edge[current_edge].vertex()->id();
         //~ cout<<" ~ "<<current_vertex;
         if (vertex[current_vertex].rigging_group == current_bone) {
            //~ cout<<" ~ "<<current_bone;
            if ((vertex[current_vertex].assigned)) {
               current_edge = edge[edge[current_edge].opposite()->hedgeid].next()->hedgeid;
               edge_ant = edge[current_edge].next()->hedgeid;
            }
            else {
               //~ ++contador;
               vertex[current_vertex].assigned = true;
               vertex[current_vertex].next_in_component = last_vertex_in_component;
               
               //~ cout<<"<::>vertex[current_vertex]: "<<current_vertex<<" : "<<vertex[current_vertex].next_in_component<<" : "<<vertex[current_vertex].rigging_group<<endl;
               
               last_vertex_in_component = current_vertex;
               lista[n_listed].e_ini = edge[edge[edge_ant].opposite()->hedgeid].next()->hedgeid;
               current_edge = edge[edge[current_edge].opposite()->hedgeid].next()->hedgeid;
               
               edge_ant = edge[current_edge].next()->hedgeid;
               lista[n_listed].e_term = edge[edge_ant].opposite()->hedgeid;
               
               n_listed = (n_listed%lista_size) + 1;
            }
         }
         else {
            //~ cout<<" ~ "<<n_listed;
            int vparent = edge[edge[current_edge].opposite()->hedgeid].vertex()->id();
            Number distance_to_skeleton = length(current_edge) + vertex[vparent].dClosestJoint;
            //~ if (vertex[current_vertex].curr_comp < closest_components) {
               //~ if (vertex[current_vertex].dClosestJoint > distance_to_skeleton) {
                  //~ vertex[current_vertex].curr_comp = current_component;
                  //~ vertex[current_vertex].dClosestJoint = distance_to_skeleton;
                  //~ int edge_ini = edge[edge[edge_ant].opposite()->hedgeid].next()->hedgeid;
                  //~ current_edge = edge[edge[current_edge].opposite()->hedgeid].next()->hedgeid;
                  //~ edge_ant = edge[current_edge].next()->hedgeid;
                  //~ int edge_term = edge[edge_ant].opposite()->hedgeid;
                  
                  //~ global_stack_element record(current_bone, current_vertex, edge_ini, edge_term, 
                                             //~ distance_to_skeleton, vertex[vparent].dClosestJoint, 
                                             //~ current_component);
                  //~ global_stack.push(record);
                  //~ ++contadorpilha;
               //~ }
               //~ else {
                  //~ current_edge = edge[edge[current_edge].opposite()->hedgeid].next()->hedgeid;
                  //~ edge_ant = edge[current_edge].next()->hedgeid;
               //~ }
            //~ }
            //~ else {
            
               //~ if (vertex[current_vertex].dClosestJoint > distance_to_skeleton) {
                  //~ vertex[current_vertex].curr_comp = current_component;
                  //~ vertex[current_vertex].dClosestJoint = distance_to_skeleton;
               //~ }
               int edge_ini = edge[edge[edge_ant].opposite()->hedgeid].next()->hedgeid;
               current_edge = edge[edge[current_edge].opposite()->hedgeid].next()->hedgeid;
               edge_ant = edge[current_edge].next()->hedgeid;
               int edge_term = edge[edge_ant].opposite()->hedgeid;
               
               global_stack_element record(current_bone, current_vertex, edge_ini, edge_term, 
                                           distance_to_skeleton, vertex[vparent].dClosestJoint, 
                                           current_component);
               global_stack.push(record);
               //~ ++contadorpilha;
               //~ cout<<" : "<<contadorpilha;
            //~ }
         }
      }
      while (current_edge != lista[n_explored].e_term);
      //~ cout<<">>>>>"<<endl;
      n_explored = (n_explored%lista_size) + 1;
   }
   
   //~ cout<<">>>>> "<<current_component<<" <:> "<<contador<<" <> "<<last_vertex_in_component<<" contadorpilha: "<<contadorpilha<<endl;
   return(last_vertex_in_component);
}

int Clustering::Initial_vertex_invisible_bonejoint(int v, const Vector3 &vd, const Vector3 &c) {
   //~ cout<<"Initial_vertex_invisible() begin: "<<v<<endl;
   
   MeshWithSkeleton::Vertex_iterator vertex = model->vertices_begin();
   MeshWithSkeleton::Halfedge_iterator edge = model->halfedges_begin();
   
   Vector3 current_edge_vector[2];
   
   const Point3 &p = vertex[v].point();
   //~ Vector3 vd = (model->skeleton->getBoneProjector(&p[0], vertex[v].rigging_group))*(-1.0);
   //~ Vector3 c = Vector3(p[0],p[1],p[2]) + vd;
   
   Number vd_norm_2 = vd*vd;
   
   int current_edge = vertex[v].halfedge()->opposite()->hedgeid;
   int current_vertex_id = edge[current_edge].vertex()->id();
   
   const Point3 &current_neighbor = vertex[current_vertex_id].point();
   current_edge_vector[0] = Vector3(current_neighbor[0]-p[0], current_neighbor[1] - p[1], current_neighbor[2] - p[2]);
   current_edge = edge[edge[current_edge].opposite()->hedgeid].next()->hedgeid;
   
   //int current_maxedge = -1;
   Number distance_to_v = length(current_edge);
   Number current_max = - 1E8;
   int terminal_edge = current_edge;
   int i_current = 1;
   
   int sgn_min, sgn_min_1, sgn_min_2, sgn_min_concavity;
   bool flag_interior_min, flag_just_changed;
   Vector3 previous_min_edge;
   
   stack stack_search;
   do {
      current_vertex_id = edge[current_edge].vertex()->id();
      const Point3 &current_neighbor = vertex[current_vertex_id].point();
      current_edge_vector[i_current] = Vector3(current_neighbor[0]-p[0], current_neighbor[1]-p[1], current_neighbor[2] - p[2]);
      distance_to_v = length(current_edge);
      
      //~ cout<<"*** current_vertex_id:"<<current_vertex_id<<endl;
      //~ cout<<"*** current_edge:"<<current_edge<<endl;
      //~ cout<<"*** current_edge:"<<current_edge<<endl;
      
      Vector3 current_normal = cross_product(current_edge_vector[i_current],current_edge_vector[1-i_current]);
      int sgn_tmp = CGAL::sign(current_normal*vd);
      
      //~ cout<<" www :"<<current_normal*vd<<endl;
      //~ cout<<" sign www :"<<CGAL::sign(current_normal*vd)<<endl;
      current_normal = current_normal*(-sgn_tmp);
      if (interTriSeg((current_edge_vector[0])*(1e4),(current_edge_vector[1])*(1e4),
                       current_normal*(-1e4), vd)) {
         //~ cout<<"***interTriSeg() true"<<endl;
         flag_just_changed = false;
         Number current_normal_2 = current_normal*current_normal;
         Number prod_esc_temp  = vd * current_normal;
         Number dist_tmp = sqrt(vd_norm_2 - (prod_esc_temp * prod_esc_temp)/(current_normal_2));
         if (dist_tmp > current_max) {
            current_max = dist_tmp;
            flag_interior_min = true;
            sgn_min = sgn_tmp;
            //~ cout<<"*** current_min: "<<current_min<<endl;
            //~ cout<<"*** sgn_min: "<<sgn_min<<endl;
         }
         
      }
      else {
         //~ cout<<"***interTriSeg() false"<<endl;
         Number current_edge_norm = sqrt(current_edge_vector[i_current] * current_edge_vector[i_current]);
         Number prod_esc_tmp = vd * current_edge_vector[i_current];
         Number dist_tmp = prod_esc_tmp /current_edge_norm;
         if (dist_tmp > current_max) {
            current_max = dist_tmp;
            flag_interior_min = false;
            flag_just_changed =true;
            sgn_min_1 = sgn_tmp;
            previous_min_edge = current_edge_vector[1-i_current];
            //~ cout<<"*** current_min: "<<current_min<<endl;
            //~ cout<<"*** sgn_min_1: "<<sgn_min<<endl;
         }
         else {
            if (flag_just_changed) {
               flag_just_changed = false;
               sgn_min_2 =  sgn_tmp;
               sgn_min_concavity = -sgn_tmp*CGAL::sign(previous_min_edge*current_normal);
               //~ cout<<"*** sgn_min_2: "<<sgn_min_2<<endl;
               //~ cout<<"*** sgn_min_concavity: "<<sgn_min_concavity<<endl;
            }
         }
      }
      
      int current_vertex = current_vertex_id;
      i_current = 1- i_current;
      vertex[current_vertex].curr_comp_inter = v;
      int edge_ant = edge[current_edge].next()->hedgeid;
      int edge_ini = edge[edge[edge_ant].opposite()->hedgeid].next()->hedgeid;
      int edge_term = edge[current_edge].opposite()->hedgeid;
      current_edge = edge[edge[current_edge].opposite()->hedgeid].next()->hedgeid;
      edge_ant = edge[current_edge].next()->hedgeid;
      stack_element record(current_vertex_id, edge_ini, edge_term, distance_to_v);
      stack_search.push(record);
      
      //~ if (v == 1679) {
         //~ plistavisitados->push_back(current_vertex_id);
      //~ }
      
   } while(current_edge != terminal_edge);
   
   if(flag_just_changed) {
      current_vertex_id =edge[current_edge].vertex()->id();
      //~ Number *current_neighbor =&model->vpositions[current_vertex_id];
      const Point3 &current_neighbor = vertex[current_vertex_id].point();
      current_edge_vector[i_current] = Vector3(current_neighbor[0]-p[0],
      current_neighbor[1]-p[1],current_neighbor[2]-p[2]);
      Vector3 current_normal =cross_product(current_edge_vector[i_current],
                                    current_edge_vector[1 - i_current]);
      sgn_min_2 = CGAL::sign(current_normal*vd);
      sgn_min_concavity = CGAL::sign(previous_min_edge*current_normal);
      //~ cout<<"*** depois do do"<<endl;
      //~ cout<<"*** sgn_min_2: "<<sgn_min_2<<endl;
      //~ cout<<"*** sgn_min_concavity: "<<sgn_min_concavity<<endl;
   }
   
   
   //~ cout<<":: current_min: "<<current_min<<endl;
   //~ cout<<":: flag_interior_min: "<<flag_interior_min<<endl;
   if(flag_interior_min) {
      //~ cout<<"flag_interior_min true"<<endl;
     if(sgn_min >= 0) { 
        //~ cout<<"%%% flag_interior_min "<<endl; 
        return v;}
   }
   else {
      //~ cout<<"flag_interior_min false"<<endl;
      //~ cout<<"sgn_min_concavity: "<<sgn_min_concavity<<endl;
      //~ cout<<"sgn_min_1: "<<sgn_min_1<<endl;
      //~ cout<<"sgn_min_2: "<<sgn_min_2<<endl;
      if(sgn_min_concavity >= 0) {
        if((sgn_min_1 >= 0) and (sgn_min_2 >= 0)) { 
           //~ cout<<"%%% sgn_min_concavity>0 "<<endl; 
           return v;}
      }
      else {
         if((sgn_min_1 >= 0) or (sgn_min_2 >= 0)) { 
            //~ cout<<"%%% sgn_min_concavity<=0 "<<endl; 
            return v;}
      }
   }
   
   while (!(stack_search.empty())) {
      //~ cout<<"whileeeeeeeeeeeeeee"<<endl;
      stack_element record = stack_search.top();
      stack_search.pop();
      
      const Point3 &p = vertex[record.vertex].point();
      //~ const Number *p = &model->vpositions[record.vertex];
      Vector3 c0_p0(c[0]-p[0], c[1]-p[1], c[2]-p[2]);
      current_edge = record.e_init;
      int edge_ant = edge[current_edge].next()->hedgeid;
      int vertex_ant_id = edge[edge_ant].vertex()->id();
      //~ const Number *q = &model->vpositions[vertex_ant_id];
      const Point3 &q = vertex[vertex_ant_id].point();
      current_edge_vector[0] = Vector3(q[0]-p[0], q[1]-p[1], q[2]-p[2]);
      i_current = 1;
      do {
         //~ cout<<"doooooooooooooooo: "<<current_edge<<" <> "<<record.e_term<<endl;
         
         //~ if (current_edge == 9565) {
            //~ cout<<"::: vertex: "<<edge[current_edge].vertex()->id()<<endl;
            //~ do {
               //~ cout<<"::: current_edge: "<<current_edge<<endl;
               //~ current_edge = edge[edge[current_edge].opposite()->hedgeid].next()->hedgeid;
            //~ } while (current_edge != 9565);
            //~ current_edge = 9565;
         //~ }
         
         current_vertex_id = edge[current_edge].vertex()->id();
         //~ const Number *q = &model->vpositions[current_vertex_id];
         const Point3 &q = vertex[current_vertex_id].point();
         current_edge_vector[i_current] = Vector3(q[0]-p[0], q[1]-p[1], q[2]-p[2]);
         int current_vertex = current_vertex_id;
         bool flag = interTriSeg(current_edge_vector[0], current_edge_vector[1], vd, c0_p0);
         //~ if (flag) cout<<" 2 cheguei TRUEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE"<<endl;
         //~ else      cout<<" 2 cheguei FALSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSES"<<endl;
         if (flag) {
            //~ cout<<"%%% Initial_vertex_invisible() end (return)"<<current_vertex<<endl;
            return current_vertex;
         }
         i_current = 1 - i_current;
         Number distance_to_v = record.dist + length(current_edge);
         if((vertex[current_vertex].curr_comp_inter != v) and (distance_to_v < inter_test_limit)) {
            vertex[current_vertex].curr_comp_inter = v;
            int edge_ini = edge[edge[edge_ant].opposite()->hedgeid].next()->hedgeid;
            int edge_term = edge[current_edge].opposite()->hedgeid;
            current_edge = edge[edge[current_edge].opposite()->hedgeid].next()->hedgeid;
            edge_ant = edge[current_edge].next()->hedgeid;
            stack_element record(current_vertex_id, edge_ini, edge_term, distance_to_v);
            stack_search.push(record);
         }
         else {
           current_edge = edge[edge[current_edge].opposite()->hedgeid].next()->hedgeid;
           edge_ant = edge[current_edge].next()->hedgeid;
         }
     }
     while (current_edge != record.e_term);
   }
   //~ cout<<"%%% Initial_vertex_invisible() end -1: "<<v<<endl;
   cout<<"%%% Initial_vertex_invisible_bone() end -1: "<<v<<endl;
   return -1;
}

int Clustering::Vertex_invisible_bonejoint(int v, int xtriang_vertex, const Vector3 &vd, const Vector3 &c) {
   //~ cout<<"Vertex_invisible(int v, int xtriang_vertex) begin : "<<v<<" "<<xtriang_vertex<<endl;
   
   MeshWithSkeleton::Vertex_iterator vertex = model->vertices_begin();
   MeshWithSkeleton::Halfedge_iterator edge = model->halfedges_begin();
   int vid = vertex[v].id();
   int vidx = vertex[xtriang_vertex].id();
   
   //~ const Point3 &p = vertex[vid].point();
   Vector3 current_edge_vector[2];
   
   Number normvd = sqrt(vd*vd);
   if (fabs(normvd) < 1e-8) { return -1;}
   
   Number normvd_1 = 1.0/normvd;
   
   Vector3 vd_norm = normvd_1*vd;
   int terminal_edge =vertex[xtriang_vertex].halfedge()->opposite()->hedgeid;
   int initial_edge = terminal_edge;
   int i_current = 1;
   
   Stack_2_Distances stack_search;
   stack_2_distances record(vidx, initial_edge, terminal_edge, 0, 0);
   stack_search.push(record);
   
   while (!(stack_search.empty())) {
      record = stack_search.top();
      stack_search.pop();
      //~ const Number *p = &model->vpositions[record.vertex];
      const Point3 &p = vertex[record.vertex].point();
      Vector3 c0_p0(c[0]-p[0], c[1]-p[1], c[2]-p[2]);
      int current_edge = record.e_init;
      int edge_ant = edge[current_edge].next()->hedgeid;
      int vertex_ant_id = edge[edge_ant].vertex()->id();
      if (vertex_ant_id == vid) {
         current_edge = edge[edge[current_edge].opposite()->hedgeid].next()->hedgeid;
         edge_ant = edge[current_edge].next()->hedgeid;
         vertex_ant_id = edge[edge_ant].vertex()->id();
      }
      //~ const Number *q = &model->vpositions[vertex_ant_id];
      const Point3 &q = vertex[vertex_ant_id].point();
      current_edge_vector[0] = Vector3(q[0]-p[0], q[1]-p[1], q[2]-p[2]);
      i_current = 1;
      do {
         int current_vertex_id = edge[current_edge].vertex()->id();
         //~ const Number *q = &model->vpositions[current_vertex_id];
         const Point3 &q = vertex[current_vertex_id].point();
         current_edge_vector[i_current] = Vector3(q[0]-p[0], q[1]-p[1], q[2]-p[2]);
         int current_vertex = current_vertex_id;
         if (current_vertex_id != vid) {
            bool flag = interTriSeg(current_edge_vector[0], current_edge_vector[1], vd, c0_p0);
            //~ if (flag) cout<<"cheguei TRUEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE: "<<current_vertex<<endl;
            //~ else      cout<<"cheguei FALSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSES"<<endl;
            if (flag) return current_vertex;
            Number distance_to_xv= record.dist_to_xv + length(current_edge);
            if ((vertex[current_vertex].curr_comp_inter != v) and (distance_to_xv < inter_test_limit)) {
               vertex[current_vertex].curr_comp_inter = v;
               int edge_ini = edge[edge[edge_ant].opposite()->hedgeid].next()->hedgeid;
               int edge_term = edge[current_edge].opposite()->hedgeid;
               //~ current_edge = edge[edge[current_edge].opposite()->hedgeid].next()->hedgeid;
               //~ edge_ant = edge[current_edge].next()->hedgeid;
               Vector3 temp_vector = c0_p0 - current_edge_vector[i_current];
               temp_vector = temp_vector - vd_norm*(temp_vector*vd_norm);
               Number order_of_stack = temp_vector*temp_vector;
               stack_2_distances record(current_vertex_id, edge_ini, edge_term, distance_to_xv, order_of_stack);
               stack_search.push(record);
               //~ continue;
            }
         }
         current_edge = edge[edge[current_edge].opposite()->hedgeid].next()->hedgeid;
         edge_ant = edge[current_edge].next()->hedgeid;
         i_current = 1 - i_current;
      }
      while (current_edge != record.e_term);
   }
   
   //~ cout<<"Vertex_invisible() end: (retornando -1)"<<v<<" "<<xtriang_vertex<<endl;
   return -1;
}

void Clustering::propagate_primary_skin() {
   
   MeshWithSkeleton::Vertex_iterator   vertex = model->vertices_begin();
   MeshWithSkeleton::Halfedge_iterator edge = model->halfedges_begin();
   
   Number  distance_to_skeleton;
   global_stack_element record;
   int current_edge, edge_ant, current_vertex;
   int comp_list_size = component_list.size()-1;
   
   while (!(global_stack.empty())) {
      record = global_stack.top();
      global_stack.pop();
      
      if (component_list[record.component] == -1)      { /*cout<<"1er continue"<<endl;*/ continue; }
      if ((component_list[vertex[record.vertex].curr_comp] == -1) or 
         ((vertex[record.vertex].dClosestJoint >= record.dist_to_skel) and model->skeleton->onlyvisible[record.bone])) {
            
         vertex[record.vertex].curr_comp = comp_list_size;
         vertex[record.vertex].dClosestJoint = record.dist_to_skel;
         vertex[record.vertex].dPrimSkinRoot = record.dist_to_prim_skin;
         vertex[record.vertex].rigging_group = record.bone;
         current_edge = record.e_init;
         edge_ant = edge[current_edge].next()->hedgeid;
         do {
            current_vertex = edge[current_edge].vertex()->id();
            distance_to_skeleton = record.dist_to_skel + length(current_edge);
            if (vertex[current_vertex].dClosestJoint > distance_to_skeleton or 
               (component_list[vertex[current_vertex].curr_comp] == -1)) {
               vertex[current_vertex].dClosestJoint = distance_to_skeleton;
               vertex[current_vertex].curr_comp = comp_list_size;
               int edge_ini = edge[edge[edge_ant].opposite()->hedgeid].next()->hedgeid;
               current_edge = edge[edge[current_edge].opposite()->hedgeid].next()->hedgeid;
               edge_ant = edge[current_edge].next()->hedgeid;
               int edge_term = edge[edge_ant].opposite()->hedgeid;
               global_stack_element newrecord(record.bone, current_vertex, edge_ini, edge_term, 
                                           distance_to_skeleton, record.dist_to_prim_skin, comp_list_size);
               global_stack.push(newrecord);
            }
            else {
               current_edge = edge[edge[current_edge].opposite()->hedgeid].next()->hedgeid;
               edge_ant = edge[current_edge].next()->hedgeid;
            }
         }
         while (current_edge != record.e_term);
      }
   }
}

void Clustering::getClosestJoint(MeshWithSkeleton::Vertex_handle vertex) {
   
   int v = vertex->id();
   const Point3 &p = vertex->point();
   
   Vector3 u1 = p - curr_bas_joint;
   Vector3 u2 = p - current_basis;
   Number D_to_i1 = sqrt(u1*u1);
   Number D_to_basis = sqrt(u2*u2);
   if ((Dbasis_to_i2 - D_to_basis) < D_to_i1) {
      int i;
      Dbasis_to_i2 = 1e8;
      int iterm = i1joint;
      for (i=0; i<iterm; ++i) {
         const Point3 &c = model->skeleton->joints[i];
         Vector3 vd = p - c;
         Number d = sqrt(vd*vd);
         if (d < Dbasis_to_i2){
            if (d < D_to_i1) {
               Dbasis_to_i2 = D_to_i1;
               D_to_i1 = d;
               i1joint = i;
            }
            else{
               Dbasis_to_i2 = d;
            }
         }
      }
      i++;
      for (unsigned int j=i; j<model->skeleton->nJoints(); ++j) {
         const Point3 &c = model->skeleton->joints[j];
         Vector3 vd = p - c;
         Number d = sqrt(vd*vd);
         if (d < Dbasis_to_i2){
            if (d < D_to_i1) {
               Dbasis_to_i2 = D_to_i1;
               D_to_i1 = d;
               i1joint = j;
            }
            else {
               Dbasis_to_i2 = d;
            }
         }
      }
      curr_bas_joint = model->skeleton->joints[i1joint];
      current_basis = p;
   }
   
   vertex->rigging_group = i1joint;
   vertex->dClosestJoint = D_to_i1;
   if (D_to_i1 < model->skeleton->min_distance_vertex[i1joint]) {
      model->skeleton->min_distance_vertex[i1joint] = D_to_i1;
      model->skeleton->closest_vertex[i1joint] = v;
   }
}

void Clustering::getClosestVisibleJoint(MeshWithSkeleton::Vertex_handle vertex) {
   
   const Point3 &p = vertex->point();
   const Vector3 &n = vertex->normal();
   
   const Point3 &c = model->skeleton->joints[vertex->rigging_group];
   Vector3 vd = p - c;
   if (vd*n <= 0) {
      int i;
      Number D1=1e8;
      vertex->dClosestJoint = 1e8;
      vertex->rigging_group = -1;
      int rigginggroup = vertex->rigging_group;
      for (i=0; i<rigginggroup; i++) {
         const Point3 &c = model->skeleton->joints[i];
         Vector3 vdd = p - c;
         Number d = sqrt(vdd*vdd);
         if (vdd*n > 0) {
            if (d<D1) {
               vertex->dClosestJoint = d;
               vertex->rigging_group = i;
               D1 = d;
            }
         }
      }
      i++;
      for (unsigned int j=i; j<model->skeleton->nJoints(); j++) {
         const Point3 &c = model->skeleton->joints[j];
         Vector3 vdd = p - c;
         Number d = sqrt(vdd*vdd);
         if (vdd*n > 0) {
            if (d<D1) {
               vertex->dClosestJoint = d;
               vertex->rigging_group = j;
               D1 = d;
            }
         }
      }
   }
}

void Clustering::Rigging_Validation() {
   
   cout<<"Rigging_Validation() ..."<<endl;
   int modified_edge_list[50000];
   int num_open = 0;
   MeshWithSkeleton::Halfedge_iterator edge = model->halfedges_begin();
   MeshWithSkeleton::Vertex_iterator   vertex = model->vertices_begin();
   for(int i=0; i< model->size_of_halfedges(); i++) {
      int pointed_vertex = edge[i].vertex()->id();
      int opposite_edge = edge[i].opposite()->hedgeid;
      int pointer_vertex = edge[opposite_edge].vertex()->id();
      Number dist_temp = vertex[pointer_vertex].dClosestJoint + length(i);
      int bone_temp = vertex[pointer_vertex].rigging_group;
      if ((dist_temp < vertex[pointed_vertex].dClosestJoint) and model->skeleton->onlyvisible[bone_temp]) {
         cout<<"i: "<<i<<" : "<<pointer_vertex<<endl;
         vertex[pointed_vertex].dClosestJoint = dist_temp;
         vertex[pointed_vertex].rigging_group = bone_temp;
         vertex[pointed_vertex].tclosest_bone = vertex[pointer_vertex].tclosest_bone;
         int current_edge = opposite_edge;
         do {
            num_open++;
            modified_edge_list[num_open] = current_edge;
            current_edge = edge[edge[current_edge].opposite()->hedgeid].next()->hedgeid;
         } while(current_edge != opposite_edge);
      }
   }
   
   cout<<"Rigging_Validation() ... second step: "<<num_open<<endl;
   int num_closed = 0;
   while(num_closed < num_open) {
      int current_edge = modified_edge_list[num_closed] ;
      int pointed_vertex = edge[current_edge].vertex()->id();
      int opposite_edge = edge[current_edge].opposite()->hedgeid;
      int pointer_vertex = edge[opposite_edge].vertex()->id();
      Number dist_temp = vertex[pointer_vertex].dClosestJoint + length(current_edge);
      int bone_temp = vertex[pointer_vertex].rigging_group;
      if (dist_temp < vertex[pointed_vertex].dClosestJoint and
         model->skeleton->onlyvisible[bone_temp]) {
         //~ cout<<"num_closed: "<<num_closed<<" : "<<pointer_vertex<<endl;
         vertex[pointed_vertex].dClosestJoint = dist_temp;
         vertex[pointed_vertex].rigging_group = bone_temp;
         vertex[pointed_vertex].tclosest_bone = vertex[pointer_vertex].tclosest_bone;
         current_edge = opposite_edge;
         do {
            num_open++;
            modified_edge_list[num_open] = current_edge;
            current_edge = edge[edge[current_edge].opposite()->hedgeid].next()->hedgeid;
         } while(current_edge != opposite_edge);
      }
      num_closed++;
   }
   cout<<"Rigging_Validation() ... end"<<endl;
}
